#include "pid.h"
#include "esp_log.h"


#define LOG_INFO ESP_LOG_INFO
static const char *TAG = "app_main";







/**
 * @brief pid初始化
 * 
 * @param g_speed_pid PID结构体变量地址
 * @return int 判断是否成功
 */
int pid_init(PID_TypeDef *g_speed_pid)
{
    esp_log_level_set(TAG, LOG_INFO);

    if(g_speed_pid == NULL)
    {
        ESP_LOGE(TAG,"Passed null pointer!");
        return PID_ERROR;
    }

    g_speed_pid->SetPoint = 0;       /* 设定目标值 */
    g_speed_pid->ActualValue = 0.0;  /* 期望输出值 */
    g_speed_pid->SumError = 0.0;     /* 积分值 */
    g_speed_pid->Error = 0.0;        /* Error[1] */
    g_speed_pid->LastError = 0.0;    /* Error[-1] */
    g_speed_pid->PrevError = 0.0;    /* Error[-2] */
    g_speed_pid->Proportion = 0;    /* 比例常数 Proportional Const */
    g_speed_pid->Integral = 0;      /* 积分常数 Integral Const */
    g_speed_pid->Derivative = 0;    /* 微分常数 Derivative Const */ 

    return PID_NOERROR;
}

/**
 * @brief 增量式pid计算
 * 
 * @param PID PID结构体变量地址
 * @param Feedback_value 
 * @return int 
 */
int increment_pid_ctrl(PID_TypeDef *PID,float Feedback_value)
{
    esp_log_level_set(TAG, LOG_INFO);

    if(PID == NULL)
    {
        ESP_LOGE(TAG,"Passed null pointer!");
        return PID_ERROR;
    }

    PID->Error = (float)(PID->SetPoint - Feedback_value); /* 计算偏差 */

    PID->ActualValue += (PID->Proportion * (PID->Error - PID->LastError))                          /* 比例环节 */
                        + (PID->Integral * PID->Error)                                             /* 积分环节 */
                        + (PID->Derivative * (PID->Error - 2 * PID->LastError + PID->PrevError));  /* 微分环节 */

    PID->PrevError = PID->LastError; /* 存储偏差，用于下次计算 */
    PID->LastError = PID->Error;

    return PID_NOERROR;
}

int position_pid_ctrl(PID_TypeDef *PID,float Feedback_value)
{
    esp_log_level_set(TAG, LOG_INFO);

    if(PID == NULL)
    {
        ESP_LOGE(TAG,"Passed null pointer!");
        return PID_ERROR;
    }

    PID->Error = (float)(PID->SetPoint - Feedback_value); /* 计算偏差 */

    PID->SumError += PID->Error;
    PID->ActualValue = (PID->Proportion * PID->Error)                       /* 比例环节 */
                       + (PID->Integral * PID->SumError)                    /* 积分环节 */
                       + (PID->Derivative * (PID->Error - PID->LastError)); /* 微分环节 */
    PID->LastError = PID->Error;

    ESP_LOGI("");

    return PID_NOERROR;
}





